#include "ros/ros.h"
#include "std_msgs/String.h"
#include "std_msgs/Int32.h"
#include <iostream>
#include <string>
#include <algorithm>
#include <iterator>
#include <vector>
#include <locale>
#include <cstring>
#include <sstream>

//topic
ros::Subscriber sub;

ros::Publisher master_pub_ca0;
ros::Publisher master_pub_ca1;
ros::Publisher master_pub_ca2;
ros::Publisher master_pub_ca5;
ros::Publisher master_pub_te;
ros::Publisher master_pub_ve;
ros::Publisher master_pub_embedded;
ros::Publisher master_pub_risposta;
ros::Publisher master_pub_polling_master;
ros::Publisher master_pub_fan;

ros::Publisher potenza_pub_perc_master;
ros::Publisher potenza_pub_carica_master;
ros::Publisher potenza_pub_stato_master;
ros::Publisher potenza_pub_perc_potenza;
ros::Publisher potenza_pub_carica_potenza;
ros::Publisher potenza_pub_stato_potenza;
ros::Publisher potenza_pub_perc_motori;
ros::Publisher potenza_pub_carica_motori;
ros::Publisher potenza_pub_stato_motori;
ros::Publisher potenza_pub_perc_sensori;
ros::Publisher potenza_pub_carica_sensori;
ros::Publisher potenza_pub_stato_sensori;
ros::Publisher potenza_pub_V_master;
ros::Publisher potenza_pub_I_master;
ros::Publisher potenza_pub_V_power;
ros::Publisher potenza_pub_I_power;
ros::Publisher potenza_pub_V_motori;
ros::Publisher potenza_pub_I_motori;
ros::Publisher potenza_pub_V_5_sensori;
ros::Publisher potenza_pub_I_5_sensori;
ros::Publisher potenza_pub_V_3_sensori;
ros::Publisher potenza_pub_I_3_sensori;
ros::Publisher potenza_pub_I_12Ve;
ros::Publisher potenza_pub_V_5Vricarica;
ros::Publisher potenza_pub_V_24Vbatteria;
ros::Publisher potenza_pub_potenza_totale_mW;
ros::Publisher potenza_pub_allarmi;
ros::Publisher potenza_pub_dt_dati;

ros::Publisher motori_pub_vmax;
ros::Publisher motori_pub_vcl;
ros::Publisher motori_pub_vcr;
ros::Publisher motori_pub_v1;
ros::Publisher motori_pub_v2;
ros::Publisher motori_pub_v3;
ros::Publisher motori_pub_v4;
ros::Publisher motori_pub_i1;
ros::Publisher motori_pub_i2;
ros::Publisher motori_pub_i3;
ros::Publisher motori_pub_i4;
ros::Publisher motori_pub_T1;
ros::Publisher motori_pub_T2;
ros::Publisher motori_pub_T3;
ros::Publisher motori_pub_T4;
ros::Publisher motori_pub_dt_dati;
ros::Publisher motori_pub_dt_ultimo_flag_critico;
ros::Publisher motori_pub_ultimo_flag;
ros::Publisher motori_pub_mot;
ros::Publisher motori_pub_allarmi;
ros::Publisher motori_pub_mf0;
ros::Publisher motori_pub_mf1;
ros::Publisher motori_pub_mf2;
ros::Publisher motori_pub_mf3;
ros::Publisher motori_pub_sr0;
ros::Publisher motori_pub_sr1;
ros::Publisher motori_pub_sr2;
ros::Publisher motori_pub_sr3;

ros::Publisher sensori_pub_T_1_x10;
ros::Publisher sensori_pub_T_2_x10;
ros::Publisher sensori_pub_T_3_x10;
ros::Publisher sensori_pub_T_4_x10;
ros::Publisher sensori_pub_componente_x_G_x10;
ros::Publisher sensori_pub_componente_y_G_x10;
ros::Publisher sensori_pub_componente_z_G_x10;
ros::Publisher sensori_pub_allarmi;
ros::Publisher sensori_pub_T_ser_1;
ros::Publisher sensori_pub_T_ser_2;
ros::Publisher sensori_pub_T_PC;
ros::Publisher sensori_pub_T_conv_1;
ros::Publisher sensori_pub_T_conv_2;
ros::Publisher sensori_pub_T_conv_3;

struct tokens: std::ctype<char> 
{
	tokens(): std::ctype<char>(get_table()) {}
	static std::ctype_base::mask const* get_table()
	{
		typedef std::ctype<char> cctype;
		static const cctype::mask *const_rc= cctype::classic_table();

		static cctype::mask rc[cctype::table_size];
		std::memcpy(rc, const_rc, cctype::table_size * sizeof(cctype::mask));

		//rc['|'] = std::ctype_base::space; 
		rc[';'] = std::ctype_base::space; 
		return &rc[0];
	}
};


void serialCallback(const std_msgs::String::ConstPtr& msg)
{
  
  //ROS_INFO("Ricevuto messaggio: [%s]", msg->data.c_str());     //funzione che stampa a video i dati ricevuti

  std::stringstream ss(msg->data.c_str()); //crea uno string buffer dal messaggio, ss è il puntatore all'inizio.
  ss.imbue(std::locale(std::locale(), new tokens())); //imposta il token come il nuovo separatore
  std::istream_iterator<std::string> begin(ss); //itera lo stream a partire da ss
  std::istream_iterator<std::string> end; //fino alla fine e lo separa ad ogni token (- e ;)
  std::vector<std::string> vstrings(begin, end); //converte le stringhe in vettore di stringhe vstring
  //std::copy(vstrings.begin(), vstrings.end(), std::ostream_iterator<std::string>(std::cout, "\n")); //stampa a video la stringa di stringhe separate da un a capo
  
  //vstrings.at(0) //timestamp
  //vstrings.at(7) //IDS
  //vstrings.at(8) //IDD
  //vstrings.at(9) //dato

  std_msgs::String msg_string;  //definisce il messaggio stringa
  std_msgs::Int32 msg_int;  //definisce il messaggio stringa
  
  switch (atoi(vstrings.at(7).c_str()))
	{
	case 0: //master
		  switch (atoi(vstrings.at(8).c_str()))
			{
			case 1:						
				msg_int.data = atoi(vstrings.at(9).c_str());
				master_pub_ca0.publish(msg_int); //pubblicazione dato letto
				break;
			case 2:
				msg_int.data = atoi(vstrings.at(9).c_str());
				master_pub_ca1.publish(msg_int); //pubblicazione dato letto
				break;
			case 3:
				msg_int.data = atoi(vstrings.at(9).c_str());
				master_pub_ca2.publish(msg_int); //pubblicazione dato letto
				break;
			case 6:
				msg_int.data = atoi(vstrings.at(9).c_str());
				master_pub_ca5.publish(msg_int); //pubblicazione dato letto
				break;
			case 7:
				msg_int.data = atoi(vstrings.at(9).c_str());
				master_pub_te.publish(msg_int); //pubblicazione dato letto
				break;
			case 8:
				msg_int.data = atoi(vstrings.at(9).c_str());
				master_pub_ve.publish(msg_int); //pubblicazione dato letto
				break;
			case 9:
				msg_int.data = atoi(vstrings.at(9).c_str());
				master_pub_embedded.publish(msg_int); //pubblicazione dato letto
				break;
			case 10:
				msg_int.data = atoi(vstrings.at(9).c_str());
				master_pub_fan.publish(msg_int); //pubblicazione dato letto
				break;
			case 11:
				msg_int.data = atoi(vstrings.at(9).c_str());
				master_pub_polling_master.publish(msg_int); //pubblicazione dato letto
				break;
			case 12:
				msg_string.data = vstrings.at(9).c_str();
				master_pub_risposta.publish(msg_string); //pubblicazione dato letto
				break;
			default:
				ROS_INFO("Errore parsing scheda master");
				ROS_INFO("%d", atoi(vstrings.at(8).c_str()));
				break;
			}
	case 1: //potenza
		switch (atoi(vstrings.at(8).c_str()))
			{
			case 1:
				msg_int.data = atoi(vstrings.at(9).c_str());
				potenza_pub_perc_master.publish(msg_int); //pubblicazione dato letto
				break;
			case 2:
				msg_int.data = atoi(vstrings.at(9).c_str());
				potenza_pub_carica_master.publish(msg_int); //pubblicazione dato letto
				break;
			case 3:
				msg_int.data = atoi(vstrings.at(9).c_str());
				potenza_pub_stato_master.publish(msg_int); //pubblicazione dato letto
				break;
			case 4:
				msg_int.data = atoi(vstrings.at(9).c_str());
				potenza_pub_perc_potenza.publish(msg_int); //pubblicazione dato letto
				break;
			case 5:
				msg_int.data = atoi(vstrings.at(9).c_str());
				potenza_pub_carica_potenza.publish(msg_int); //pubblicazione dato letto
				break;
			case 6:
				msg_int.data = atoi(vstrings.at(9).c_str());
				potenza_pub_stato_potenza.publish(msg_int); //pubblicazione dato letto
				break;
			case 7:
				msg_int.data = atoi(vstrings.at(9).c_str());
				potenza_pub_perc_motori.publish(msg_int); //pubblicazione dato letto
				break;
			case 8:
				msg_int.data = atoi(vstrings.at(9).c_str());
				potenza_pub_carica_motori.publish(msg_int); //pubblicazione dato letto
				break;
			case 9:
				msg_int.data = atoi(vstrings.at(9).c_str());
				potenza_pub_stato_motori.publish(msg_int); //pubblicazione dato letto
				break;
			case 10:
				break;
			case 11:
				break;
			case 12:
				break;
			case 13:
				break;
			case 14:
				break;
			case 15:
				break;
			case 16:
				msg_int.data = atoi(vstrings.at(9).c_str());
				potenza_pub_perc_sensori.publish(msg_int); //pubblicazione dato letto
				break;
			case 17:
				msg_int.data = atoi(vstrings.at(9).c_str());
				potenza_pub_carica_sensori.publish(msg_int); //pubblicazione dato letto
				break;
			case 18:
				msg_int.data = atoi(vstrings.at(9).c_str());
				potenza_pub_stato_sensori.publish(msg_int); //pubblicazione dato letto
				break;
			case 19:
				msg_int.data = atoi(vstrings.at(9).c_str());
				potenza_pub_V_master.publish(msg_int); //pubblicazione dato letto
				break;
			case 20:
				msg_int.data = atoi(vstrings.at(9).c_str());
				potenza_pub_I_master.publish(msg_int); //pubblicazione dato letto
				break;
			case 21:
				msg_int.data = atoi(vstrings.at(9).c_str());
				potenza_pub_V_power.publish(msg_int); //pubblicazione dato letto
				break;
			case 22:
				msg_int.data = atoi(vstrings.at(9).c_str());
				potenza_pub_I_power.publish(msg_int); //pubblicazione dato letto
				break;
			case 23:
				msg_int.data = atoi(vstrings.at(9).c_str());
				potenza_pub_V_motori.publish(msg_int); //pubblicazione dato letto
				break;
			case 24:
				msg_int.data = atoi(vstrings.at(9).c_str());
				potenza_pub_I_motori.publish(msg_int); //pubblicazione dato letto
				break;
			case 25:
				break;
			case 26:
				break;
			case 27:
				break;
			case 28:
				break;
			case 29:
				break;
			case 30:
				break;
			case 31:
				break;
			case 32:
				break;
			case 33:
				msg_int.data = atoi(vstrings.at(9).c_str());
				potenza_pub_V_5_sensori.publish(msg_int); //pubblicazione dato letto
				break;
			case 34:
				msg_int.data = atoi(vstrings.at(9).c_str());
				potenza_pub_I_5_sensori.publish(msg_int); //pubblicazione dato letto
				break;
			case 35:
				msg_int.data = atoi(vstrings.at(9).c_str());
				potenza_pub_V_3_sensori.publish(msg_int); //pubblicazione dato letto
				break;
			case 36:
				msg_int.data = atoi(vstrings.at(9).c_str());
				potenza_pub_I_3_sensori.publish(msg_int); //pubblicazione dato letto
				break;
			case 37:
				break;
			case 38:
				msg_int.data = atoi(vstrings.at(9).c_str());
				potenza_pub_I_12Ve.publish(msg_int); //pubblicazione dato letto
				break;
			case 39:
				msg_int.data = atoi(vstrings.at(9).c_str());
				potenza_pub_V_5Vricarica.publish(msg_int); //pubblicazione dato letto
				break;
			case 40:
				break;
			case 41:
				msg_int.data = atoi(vstrings.at(9).c_str());
				potenza_pub_V_24Vbatteria.publish(msg_int); //pubblicazione dato letto
				break;
			case 42:
				msg_int.data = atoi(vstrings.at(9).c_str());
				potenza_pub_potenza_totale_mW.publish(msg_int); //pubblicazione dato letto
				break;
			case 43:
				msg_int.data = atoi(vstrings.at(9).c_str());
				potenza_pub_allarmi.publish(msg_string); //pubblicazione dato letto
				break;
			case 44:
				msg_int.data = atoi(vstrings.at(9).c_str());
				potenza_pub_dt_dati.publish(msg_int); //pubblicazione dato letto
				break; 
			default:
				ROS_INFO("Errore parsing scheda potenza");
				ROS_INFO("%d", atoi(vstrings.at(8).c_str()));
				break;
			}
		break;
	case 2: //motori
		switch (atoi(vstrings.at(8).c_str()))
			{			
			case 1:
				msg_int.data = atoi(vstrings.at(9).c_str());
				motori_pub_vmax.publish(msg_int); //pubblicazione dato letto
				break;
			case 2:
				msg_int.data = atoi(vstrings.at(9).c_str());
				motori_pub_vcl.publish(msg_int); //pubblicazione dato letto
				break;
			case 3:
				msg_int.data = atoi(vstrings.at(9).c_str());
				motori_pub_vcr.publish(msg_int); //pubblicazione dato letto
				break;
			case 4:
				msg_int.data = atoi(vstrings.at(9).c_str());
				motori_pub_v1.publish(msg_int); //pubblicazione dato letto
				break;
			case 5:
				msg_int.data = atoi(vstrings.at(9).c_str());
				motori_pub_v2.publish(msg_int); //pubblicazione dato letto
				break;
			case 6:
				msg_int.data = atoi(vstrings.at(9).c_str());
				motori_pub_v3.publish(msg_int); //pubblicazione dato letto
				break;
			case 7:
				msg_int.data = atoi(vstrings.at(9).c_str());
				motori_pub_v4.publish(msg_int); //pubblicazione dato letto
				break;
			case 8:
				msg_int.data = atoi(vstrings.at(9).c_str());
				motori_pub_i1.publish(msg_int); //pubblicazione dato letto
				break;
			case 9:
				msg_int.data = atoi(vstrings.at(9).c_str());
				motori_pub_i2.publish(msg_int); //pubblicazione dato letto
				break;
			case 10:
				msg_int.data = atoi(vstrings.at(9).c_str());
				motori_pub_i3.publish(msg_int); //pubblicazione dato letto
				break;
			case 11:
				msg_int.data = atoi(vstrings.at(9).c_str());
				motori_pub_i4.publish(msg_int); //pubblicazione dato letto
				break;
			case 12:
				msg_int.data = atoi(vstrings.at(9).c_str());
				motori_pub_T1.publish(msg_int); //pubblicazione dato letto
				break;
			case 13:
				msg_int.data = atoi(vstrings.at(9).c_str());
				motori_pub_T2.publish(msg_int); //pubblicazione dato letto
				break;
			case 14:
				msg_int.data = atoi(vstrings.at(9).c_str());
				motori_pub_T3.publish(msg_int); //pubblicazione dato letto
				break;
			case 15:
				msg_int.data = atoi(vstrings.at(9).c_str());
				motori_pub_T4.publish(msg_int); //pubblicazione dato letto
				break;
			case 16:
				msg_int.data = atoi(vstrings.at(9).c_str());
				motori_pub_dt_dati.publish(msg_int); //pubblicazione dato letto
				break;
			case 17:
				msg_int.data = atoi(vstrings.at(9).c_str());
				motori_pub_dt_ultimo_flag_critico.publish(msg_int); //pubblicazione dato letto
				break;
			case 18:
				msg_int.data = atoi(vstrings.at(9).c_str());
				motori_pub_ultimo_flag.publish(msg_int); //pubblicazione dato letto
				break;
			case 19:
				msg_int.data = atoi(vstrings.at(9).c_str());
				motori_pub_mot.publish(msg_int); //pubblicazione dato letto
				break;
			case 20:
				msg_string.data = vstrings.at(9).c_str();
				motori_pub_allarmi.publish(msg_string); //pubblicazione dato letto
				break;
			case 21:
				msg_string.data = vstrings.at(9);
				motori_pub_mf0.publish(msg_string); //pubblicazione dato letto
				break;
			case 22:
				msg_string.data = vstrings.at(9);
				motori_pub_mf1.publish(msg_string); //pubblicazione dato letto
				break;
			case 23:
				msg_string.data = vstrings.at(9);
				motori_pub_mf2.publish(msg_string); //pubblicazione dato letto
				break;
			case 24:
				msg_string.data = vstrings.at(9);
				motori_pub_mf3.publish(msg_string); //pubblicazione dato letto
				break;
			case 25:
				msg_string.data = vstrings.at(9);
				motori_pub_sr0.publish(msg_string); //pubblicazione dato letto
				break;
			case 26:
				msg_string.data = vstrings.at(9);
				motori_pub_sr1.publish(msg_string); //pubblicazione dato letto
				break;
			case 27:
				msg_string.data = vstrings.at(9);
				motori_pub_sr2.publish(msg_string); //pubblicazione dato letto
				break;
			case 28:
				msg_string.data = vstrings.at(9);
				motori_pub_sr3.publish(msg_string); //pubblicazione dato letto
				break;
			default:
				ROS_INFO("Errore parsing scheda");
				ROS_INFO("%d", atoi(vstrings.at(8).c_str()));
				break;
			}		
		break;
	case 3: //sensori
		switch (atoi(vstrings.at(8).c_str()))
			{
			case 1:
				msg_string.data = vstrings.at(9).c_str();
				sensori_pub_T_1_x10.publish(msg_int); //pubblicazione dato letto
				break;
			case 2:
				msg_string.data = vstrings.at(9).c_str();
				sensori_pub_T_2_x10.publish(msg_int); //pubblicazione dato letto
				break;
			case 3:
				msg_string.data = vstrings.at(9).c_str();
				sensori_pub_T_3_x10.publish(msg_int); //pubblicazione dato letto
				break;
			case 4:
				msg_string.data = vstrings.at(9).c_str();
				sensori_pub_T_4_x10.publish(msg_int); //pubblicazione dato letto
				break;
			case 5:
				msg_string.data = vstrings.at(9).c_str();
				sensori_pub_componente_x_G_x10.publish(msg_int); //pubblicazione dato letto
				break;
			case 6:
				msg_string.data = vstrings.at(9).c_str();
				sensori_pub_componente_y_G_x10.publish(msg_int); //pubblicazione dato letto
				break;
			case 7:
				msg_string.data = vstrings.at(9).c_str();
				sensori_pub_componente_z_G_x10.publish(msg_int); //pubblicazione dato letto
				break;
			case 8:
				msg_string.data = vstrings.at(9).c_str();
				sensori_pub_allarmi.publish(msg_string); //pubblicazione dato letto
				break;
			case 9:
				msg_int.data = atoi(vstrings.at(9).c_str());
				sensori_pub_T_ser_1.publish(msg_int); //pubblicazione dato letto
				break;
			case 10:
				msg_int.data = atoi(vstrings.at(9).c_str());
				sensori_pub_T_ser_2.publish(msg_int); //pubblicazione dato letto
				break;
			case 11:
				msg_int.data = atoi(vstrings.at(9).c_str());
				sensori_pub_T_PC.publish(msg_int); //pubblicazione dato letto
				break;
			case 12:
				msg_int.data = atoi(vstrings.at(9).c_str());
				sensori_pub_T_conv_1.publish(msg_int); //pubblicazione dato letto
				break;
			case 13:
				msg_int.data = atoi(vstrings.at(9).c_str());
				sensori_pub_T_conv_2.publish(msg_int); //pubblicazione dato letto
				break;
			case 14:
				msg_int.data = atoi(vstrings.at(9).c_str());
				sensori_pub_T_conv_3.publish(msg_int); //pubblicazione dato letto
				break;
			default:
				ROS_INFO("Errore parsing scheda");
				ROS_INFO("%d", atoi(vstrings.at(8).c_str()));
				break;
			}
		break;
	default: 
		ROS_INFO("Errore parsing nodo");
		ROS_INFO("%s", vstrings.at(7).c_str());
		ROS_INFO("%d", atoi(vstrings.at(7).c_str()));
		ROS_INFO("%s", vstrings.at(8).c_str());
		ROS_INFO("%d", atoi(vstrings.at(8).c_str()));
		ROS_INFO("%s", vstrings.at(9).c_str());
		ROS_INFO("%d", atoi(vstrings.at(9).c_str()));
		break;
	}
  
  
  //mettere la roba nel messaggio
  //msg.data = ss.str(); //accede allo stream e caccia la roba nel msg
  //ROS_INFO("%s", msg.data.c_str()); //stampa il messaggio letto + timestamp, c_str returns a pointer to an array that contains a null-terminated sequence of characters (i.e., a C-string) representing the current value of the string object.
  //master_pub.publish(msg); //invia il messaggio che deve essere compatibile con il tipo dichiarato nel advertise
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "parser");
  ros::NodeHandle n; // nodo di invio di nome parser come sopra
  ROS_INFO("Parser avviato");
 
  master_pub_ca0 = n.advertise<std_msgs::Int32>("Arduino/master/ca0", 1000);
  master_pub_ca1 = n.advertise<std_msgs::Int32>("Arduino/master/ca1", 1000);
  master_pub_ca2 = n.advertise<std_msgs::Int32>("Arduino/master/ca2", 1000);
  master_pub_ca5 = n.advertise<std_msgs::Int32>("Arduino/master/ca5", 1000);
  master_pub_te = n.advertise<std_msgs::Int32>("Arduino/master/te", 1000);
  master_pub_ve = n.advertise<std_msgs::Int32>("Arduino/master/ve", 1000);
  master_pub_embedded = n.advertise<std_msgs::Int32>("Arduino/master/embedded", 1000);
  master_pub_fan = n.advertise<std_msgs::Int32>("Arduino/master/ventola", 1000);
  master_pub_polling_master = n.advertise<std_msgs::Int32>("Arduino/master/polling_master", 1000);
  master_pub_risposta = n.advertise<std_msgs::String>("Arduino/master/risposta", 1000);

  potenza_pub_perc_master = n.advertise<std_msgs::Int32>("Arduino/potenza/perc_master", 1000);
  potenza_pub_carica_master = n.advertise<std_msgs::Int32>("Arduino/potenza/carica_master", 1000);
  potenza_pub_stato_master = n.advertise<std_msgs::Int32>("Arduino/potenza/stato_master", 1000);
  potenza_pub_perc_potenza = n.advertise<std_msgs::Int32>("Arduino/potenza/perc_potenza", 1000);
  potenza_pub_carica_potenza = n.advertise<std_msgs::Int32>("Arduino/potenza/carica_potenza", 1000);
  potenza_pub_stato_potenza = n.advertise<std_msgs::Int32>("Arduino/potenza/stato_potenza", 1000);
  potenza_pub_perc_motori = n.advertise<std_msgs::Int32>("Arduino/potenza/perc_motori", 1000);
  potenza_pub_carica_motori = n.advertise<std_msgs::Int32>("Arduino/potenza/carica_motori", 1000);
  potenza_pub_stato_motori = n.advertise<std_msgs::Int32>("Arduino/potenza/stato_motori", 1000);
  potenza_pub_perc_sensori = n.advertise<std_msgs::Int32>("Arduino/potenza/perc_sensori", 1000);
  potenza_pub_carica_sensori = n.advertise<std_msgs::Int32>("Arduino/potenza/carica_sensori", 1000);
  potenza_pub_stato_sensori = n.advertise<std_msgs::Int32>("Arduino/potenza/stato_sensori", 1000);
  potenza_pub_V_master = n.advertise<std_msgs::Int32>("Arduino/potenza/V_master", 1000);
  potenza_pub_I_master = n.advertise<std_msgs::Int32>("Arduino/potenza/I_master", 1000);
  potenza_pub_V_power = n.advertise<std_msgs::Int32>("Arduino/potenza/V_power", 1000);
  potenza_pub_I_power = n.advertise<std_msgs::Int32>("Arduino/potenza/I_power", 1000);
  potenza_pub_V_motori = n.advertise<std_msgs::Int32>("Arduino/potenza/V_motori", 1000);
  potenza_pub_I_motori = n.advertise<std_msgs::Int32>("Arduino/potenza/I_motori", 1000);
  potenza_pub_V_5_sensori = n.advertise<std_msgs::Int32>("Arduino/potenza/V_5_sensori", 1000);
  potenza_pub_I_5_sensori = n.advertise<std_msgs::Int32>("Arduino/potenza/I_5_sensori", 1000);
  potenza_pub_V_3_sensori = n.advertise<std_msgs::Int32>("Arduino/potenza/V_3_sensori", 1000);
  potenza_pub_I_3_sensori = n.advertise<std_msgs::Int32>("Arduino/potenza/V_3_sensori", 1000);
  potenza_pub_I_12Ve = n.advertise<std_msgs::Int32>("Arduino/potenza/I_12Ve", 1000);
  potenza_pub_V_5Vricarica = n.advertise<std_msgs::Int32>("Arduino/potenza/V_5Vricarica", 1000);
  potenza_pub_V_24Vbatteria = n.advertise<std_msgs::Int32>("Arduino/potenza/V_24Vbatteria", 1000);
  potenza_pub_potenza_totale_mW = n.advertise<std_msgs::Int32>("Arduino/potenza/potenza_totale_mW", 1000);
  potenza_pub_allarmi = n.advertise<std_msgs::String>("Arduino/potenza/allarmi", 1000);
  potenza_pub_dt_dati = n.advertise<std_msgs::Int32>("Arduino/potenza/dt_dati", 1000);

  motori_pub_vmax = n.advertise<std_msgs::Int32>("Arduino/motori/V_max", 1000);
  motori_pub_vcl = n.advertise<std_msgs::Int32>("Arduino/motori/V_comando_left", 1000);
  motori_pub_vcr = n.advertise<std_msgs::Int32>("Arduino/motori/V_comando_right", 1000);
  motori_pub_v1 = n.advertise<std_msgs::Int32>("Arduino/motori/V1", 1000);
  motori_pub_v2 = n.advertise<std_msgs::Int32>("Arduino/motori/V2", 1000);
  motori_pub_v3 = n.advertise<std_msgs::Int32>("Arduino/motori/V3", 1000);
  motori_pub_v4 = n.advertise<std_msgs::Int32>("Arduino/motori/V4", 1000);
  motori_pub_i1 = n.advertise<std_msgs::Int32>("Arduino/motori/I1", 1000);
  motori_pub_i2 = n.advertise<std_msgs::Int32>("Arduino/motori/I2", 1000);
  motori_pub_i3 = n.advertise<std_msgs::Int32>("Arduino/motori/I3", 1000);
  motori_pub_i4 = n.advertise<std_msgs::Int32>("Arduino/motori/I4", 1000);
  motori_pub_T1 = n.advertise<std_msgs::Int32>("Arduino/motori/T1", 1000);
  motori_pub_T2 = n.advertise<std_msgs::Int32>("Arduino/motori/T2", 1000);
  motori_pub_T3 = n.advertise<std_msgs::Int32>("Arduino/motori/T3", 1000);
  motori_pub_T4 = n.advertise<std_msgs::Int32>("Arduino/motori/T4", 1000);
  motori_pub_dt_dati = n.advertise<std_msgs::Int32>("Arduino/motori/dt_dati", 1000);
  motori_pub_dt_ultimo_flag_critico = n.advertise<std_msgs::Int32>("Arduino/motori/dt_ultimo_flag_critico", 1000);
  motori_pub_ultimo_flag = n.advertise<std_msgs::Int32>("Arduino/motori/ultimo_flag", 1000);
  motori_pub_mot = n.advertise<std_msgs::Int32>("Arduino/motori/mot", 1000);
  motori_pub_allarmi = n.advertise<std_msgs::String>("Arduino/motori/allarmi", 1000);
  motori_pub_mf0 = n.advertise<std_msgs::String>("Arduino/motori/_mf0", 1000);
  motori_pub_mf1 = n.advertise<std_msgs::String>("Arduino/motori/_mf1", 1000);
  motori_pub_mf2 = n.advertise<std_msgs::String>("Arduino/motori/_mf2", 1000);
  motori_pub_mf3 = n.advertise<std_msgs::String>("Arduino/motori/_mf3", 1000);
  motori_pub_sr0 = n.advertise<std_msgs::String>("Arduino/motori/_sr0", 1000);
  motori_pub_sr1 = n.advertise<std_msgs::String>("Arduino/motori/_sr1", 1000);
  motori_pub_sr2 = n.advertise<std_msgs::String>("Arduino/motori/_sr2", 1000); 
  motori_pub_sr3 = n.advertise<std_msgs::String>("Arduino/motori/_sr3", 1000);

  sensori_pub_T_1_x10 = n.advertise<std_msgs::Int32>("Arduino/sensori/T_1_x10", 1000);
  sensori_pub_T_2_x10 = n.advertise<std_msgs::Int32>("Arduino/sensori/T_2_x10", 1000);
  sensori_pub_T_3_x10 = n.advertise<std_msgs::Int32>("Arduino/sensori/T_3_x10", 1000);
  sensori_pub_T_4_x10 = n.advertise<std_msgs::Int32>("Arduino/sensori/T_4_x10", 1000);
  sensori_pub_componente_x_G_x10 = n.advertise<std_msgs::Int32>("Arduino/sensori/componente_x_G_x10", 1000);
  sensori_pub_componente_y_G_x10 = n.advertise<std_msgs::Int32>("Arduino/sensori/componente_y_G_x10", 1000);
  sensori_pub_componente_z_G_x10 = n.advertise<std_msgs::Int32>("Arduino/sensori/componente_z_G_x10", 1000);
  sensori_pub_allarmi = n.advertise<std_msgs::String>("Arduino/sensori/allarmi", 1000);
  sensori_pub_T_ser_1 = n.advertise<std_msgs::Int32>("Arduino/sensori/T_seriale_1", 1000);
  sensori_pub_T_ser_2 = n.advertise<std_msgs::Int32>("Arduino/sensori/T_seriale_2", 1000);
  sensori_pub_T_PC = n.advertise<std_msgs::Int32>("Arduino/sensori/T_PC", 1000);
  sensori_pub_T_conv_1 = n.advertise<std_msgs::Int32>("Arduino/sensori/T_convertitore_1", 1000);
  sensori_pub_T_conv_2 = n.advertise<std_msgs::Int32>("Arduino/sensori/T_convertitore_2", 1000);
  sensori_pub_T_conv_3 = n.advertise<std_msgs::Int32>("Arduino/sensori/T_convertitore_3", 1000);

  sub = n.subscribe("serial", 1000, serialCallback); //si iscrive al topic serial da dove arrivano tutti i dati

  ros::spin(); //will enter a loop, pumping callbacks.

  return 0;
}
